/**
  ****************************(C) COPYRIGHT 2023
  Polarbear*************************
  * @file       rm_serial_driver.cpp
  * @brief      串口通信模块
  * @note       感谢@ChenJun创建本模块并开源，
  *             现内容为北极熊基于开源模块进行修改并适配自己的车车后的结果。
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     2022            ChenJun         1. done
  *  V1.0.1     2023-12-11      Penguin         1.
  添加与rm_rune_dector_node模块连接的Client
  *  V1.0.2     2024-3-1        LihanChen       1.
  添加导航数据包，并重命名packet和相关函数
  *
  @verbatim
  =================================================================================

  =================================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2023
  Polarbear*************************
  */

#include <cstdio>
#include <cstring>
#include <ros/package.h>
#include <tf2/exceptions.h>
#include <tf2/time_cache.h>
#define CV_PI 3.1415926535897932384626433832795

#include <tf2/LinearMath/Quaternion.h>
#include <tf2/utils.h>
#include <yaml-cpp/yaml.h>

#include "serial_driver/rm_serial_driver.hpp"

namespace rm_serial_driver
{

  RMSerialDriver::RMSerialDriver(const ros::NodeHandle &nh)
      : nh_(nh), JLink_Operator_{new JLinkOperations(
                     "/opt/SEGGER/JLink/libjlinkarm.so", "STM32G474CE")}
  {

    // Detect parameter client
    detector_param_client_ = nh_.serviceClient<dynamic_reconfigure::Reconfigure>(
        "/detector/set_parameters");

    initJBlink();

    receive_timer_ = nh_.createTimer(ros::Duration(0.001),
                                     &RMSerialDriver::receiveDataNav, this);
    safety_timer_ =
        nh_.createTimer(ros::Duration(1.0), &RMSerialDriver::safetyTrigger, this);

    nav_pub = nh_.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 10);
    cmd_vel_sub_ = nh_.subscribe<geometry_msgs::Twist>(
        "/cmd_vel", 1, &RMSerialDriver::sendDataNav, this);
    odom_sub = nh_.subscribe<nav_msgs::Odometry>("/aft_mapped_to_init", 1,
                                                 &RMSerialDriver::odomCB, this);

    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(ros::Duration(2));
    tf_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_);
    last_msg_time_ = ros::Time::now();

    cmd_vel.linear.x = 0.;
    cmd_vel.linear.y = 0.;
    ROS_INFO("Start RMSerialDriver!");
  }

  RMSerialDriver::~RMSerialDriver() { JLink_Operator_->stopRTT(); }

  void RMSerialDriver::initJBlink()
  {
    JLink_Operator_->initializeJLink();
    while (JLink_Operator_->startRTT() == false)
    {
      ROS_INFO("Connecting RTT...");
      ros::Duration(1.0).sleep();
    }
  }

  void RMSerialDriver::safetyTrigger(const ros::TimerEvent &)
  {
    try
    {
      if ((ros::Time::now() - last_msg_time_).toSec() > 0.2)
      {
        if (!JLink_Operator_->ifHaveUSB())
        {
          stop_jb_read_write = 1;
          ROS_INFO("Connection lost.");
          // 尝试重连
          while (!JLink_Operator_->ifHaveUSB())
          {
            ROS_INFO("Reconnecting...");
          }
          JLink_Operator_->stopRTT();
          JLink_Operator_->close();
          JLink_Operator_->reInit();
          while (JLink_Operator_->startRTT() == 0)
          {
            ROS_INFO("Reconnecting RTT...");
          }
          stop_jb_read_write = 0;
        }
      }
    }
    catch (tf2::TransformException &ex)
    {
      ROS_WARN("transform searched has lost: %s", ex.what());
    }
  }

  void RMSerialDriver::receiveDataNav(const ros::TimerEvent &)
  {
    char stm[24];
    if (stop_jb_read_write == 1)
    {
      return;
    }

    bytes_read = JLink_Operator_->readRTT(1, stm, sizeof(stm));
    if (bytes_read > 0)
    {
      PacketHeader *header = reinterpret_cast<PacketHeader *>(stm);
      // 根据包类型处理数据
      switch (header->packet_type)
      {
      case PacketTypeEnum::ENUM_PACKET_SENTRY_ACCEPT_STATUS_DATA:
      {
        NavAcceptStatusData *nav_msg =
            reinterpret_cast<NavAcceptStatusData *>(stm + sizeof(PacketHeader));
        geometry_msgs::PoseStamped target_goal;
        target_goal.header.frame_id = "camera_init";
        target_goal.pose.position.x = nav_msg->x;
        target_goal.pose.position.y = nav_msg->y;
        target_goal.pose.position.z = nav_msg->z;
        target_goal.pose.orientation.w = 1;
        target_goal.header.stamp = ros::Time::now();
        last_msg_time_ = ros::Time::now();
        nav_pub.publish(target_goal);
        break;
      }
      default:
      {
        ROS_ERROR("unknown packet type: %d", header->packet_type);
        break;
      }
      }
    }
  }

  void RMSerialDriver::sendDataNav(const geometry_msgs::TwistConstPtr &velPtr)
  {
    static int count = 0;
    if (stop_jb_read_write == 1)
    {
      return;
    }
    cmd_vel = *velPtr;
    float vx_mps = cmd_vel.linear.x;
    float vy_mps = cmd_vel.linear.y;
    float vw_rpm = 0;
    float current_yaw = tf2::getYaw(odom.pose.pose.orientation) * 180 / M_PI;
    // std::cout << current_yaw << std::endl;
    if ((((int)odom.pose.pose.position.x) * ((int)odom.pose.pose.position.x) +
         ((int)odom.pose.pose.position.y) * ((int)odom.pose.pose.position.y)) >= 2500)
    {
      vx_mps = 0;
      vy_mps = 0;
    }
    rm_serial_driver::SendPacketNav packet(vx_mps, vy_mps, vw_rpm, odom.pose.pose.position.x,
                                           odom.pose.pose.position.y, current_yaw, 0);
    packet.packet_type =
        PacketTypeEnum::ENUM_PACKET_NAV_DATA;
    packet.data_len = sizeof(packet) - sizeof(PacketHeader);
    // std::cout << "vx_mps: " << packet.vx_mps << std::endl;
    char *header_p = (char *)&packet;
    for (int i = 0; i < sizeof(PacketHeader) / 2 - 1; i++)
    {
      packet.checksum += *(uint16_t *)(header_p + 2 * i);
    }
    packet.checksum += calChecksum((char *)(&packet.vx_mps), packet.data_len);
    bytes_write = JLink_Operator_->writeRTT(1, reinterpret_cast<char *>(&packet),
                                            sizeof(packet));
  }

  void RMSerialDriver::getParams()
  {
    int baud_rate; // init 波特率
    //
    serial::flowcontrol_t fc = serial::flowcontrol_t::flowcontrol_none;
    serial::parity_t pt = serial::parity_t::parity_none;
    serial::stopbits_t sb = serial::stopbits_t::stopbits_one;
    // 通信设备名称
    nh_.param<std::string>("device_name", device_name_, "");
    //
    nh_.param<int>("baud_rate", baud_rate, 0);

    std::string fc_string;
    nh_.param<std::string>("flow_control", fc_string, "");
    if (fc_string == "none")
    {
      fc = serial::flowcontrol_t::flowcontrol_none;
    }
    else if (fc_string == "hardware")
    {
      fc = serial::flowcontrol_t::flowcontrol_hardware;
    }
    else if (fc_string == "software")
    {
      fc = serial::flowcontrol_t::flowcontrol_software;
    }
    else
    {
      ROS_ERROR("The flow_control parameter must be one of: none, software, or "
                "hardware.");
    }

    std::string pt_string;
    nh_.param<std::string>("parity", pt_string, "");
    if (pt_string == "none")
    {
      pt = serial::parity_t::parity_none;
    }
    else if (pt_string == "odd")
    {
      pt = serial::parity_t::parity_odd;
    }
    else if (pt_string == "even")
    {
      pt = serial::parity_t::parity_even;
    }
    else
    {
      ROS_ERROR("The parity parameter must be one of: none, odd, or even.");
    }

    std::string sb_string;
    nh_.param<std::string>("stop_bits", sb_string, "");
    if (sb_string == "1" || sb_string == "1.0")
    {
      sb = serial::stopbits_t::stopbits_one;
    }
    else if (sb_string == "1.5")
    {
      sb = serial::stopbits_t::stopbits_one_point_five;
    }
    else if (sb_string == "2" || sb_string == "2.0")
    {
      sb = serial::stopbits_t::stopbits_two;
    }
    else
    {
      ROS_ERROR("The stop_bits parameter must be one of: 1, 1.5, or 2.");
    }

    serial_driver_->setPort(device_name_);  //
    serial_driver_->setBaudrate(baud_rate); // 设置串口通信的波特率
    serial_driver_->setParity(pt);          // 设置校验位
    serial_driver_->setFlowcontrol(fc);     // 设置数据流控制
    serial_driver_->setStopbits(sb);        // 设置终止位
  }

  void RMSerialDriver::reopenPort()
  {
    ROS_WARN("Attempting to reopen port");
    try
    {
      if (serial_driver_->isOpen())
      {
        serial_driver_->close();
      }
      serial_driver_->open();
      ROS_INFO("Successfully reopened port");
    }
    catch (const std::exception &ex)
    {
      ROS_ERROR("Error while reopening port: %s", ex.what());
      if (ros::ok())
      {
        ros::Duration(1.0).sleep();
        reopenPort();
      }
    }
  }

  void RMSerialDriver::setParam(const std::string &param_name, int value)
  {
    dynamic_reconfigure::Reconfigure srv;
    dynamic_reconfigure::IntParameter param;
    param.name = param_name;
    param.value = value;
    srv.request.config.ints.push_back(param);

    if (detector_param_client_.call(srv))
    {
      ROS_DEBUG("Successfully set parameter %s to %d", param_name.c_str(), value);
      initial_set_param_ = true;
    }
    else
    {
      ROS_ERROR("Failed to set parameter %s", param_name.c_str());
    }
  }

} // namespace rm_serial_driver
  // namespace rm_serial_driver